package pckg;

import java.io.BufferedWriter;
import java.io.File;
import java.io.FileWriter;
import java.io.IOException;

import pckg.RobotCommand.Direction;
import pckg.realRobot.RealRobot;
import pckg.realRobot.uC.UC.Side;

public class Surf {

	private double powerL, cL;
	private double powerR, cR;
	private final long t0 = 0;
	private long tEnd;
	
	private int spLstart, spRstart;
	private int spLend, spRend;
	private int spLp1, spRp1;
	private int dirL, dirR;
	
	private long accTR,accTL;
	
	private State p1EndState,p2EndState;
	private long p1EndTime,p2EndTime;
	
	private State idealStartState;
	private State idealState;
	
	
	private int[] speeds = new int[]{9,10,11,12,13,14,15,16,17,18,19,20,
										22,24,26,28,31,35,40,47,58,70,94,141,253};
	
	public RobotCommand evaluate(State start, RobotCommand idealCmd, State idealState){
		idealStartState = new State(idealState);
		idealStartState.fi = 0;
		idealStartState.vL = idealCmd.getRealV(Side.LEFT);
		idealStartState.vR = idealCmd.getRealV(Side.RIGHT);
		
		this.idealState = new State(idealStartState);
		
		int cmdSpL = idealCmd.getSpeedL();
		int cmdSpR = idealCmd.getSpeedR();

		dirL = 1;
		if (idealCmd.getDirL() == Direction.BACKWARD){
			dirL = -1;
		}
		dirR = 1;
		if (idealCmd.getDirR() == Direction.BACKWARD){
			dirR = -1;
		}
		
		int jumps = 0;
		
		RobotCommand tmpCmd = new RobotCommand(idealCmd);
		
		double errorTmp = -1;
		int bestL = -1;
		int bestR = -1;
		int actL = RealLifeProperties.vToSpeed(idealCmd.getRealV(Side.LEFT));
		int actR = RealLifeProperties.vToSpeed(idealCmd.getRealV(Side.RIGHT));
		//int actL = RealLifeProperties.vToSpeed(start.vL);
		//int actR = RealLifeProperties.vToSpeed(start.vR);
		
		double minErr = Double.MAX_VALUE;
		
		//////////////// DBG ///////////////////////
		boolean dbg = false;
		if (dbg){
			try{
				String fileName = "srfcDbg.txt";
				File file = new File(fileName);
				file.delete();
				BufferedWriter out = new BufferedWriter(new FileWriter(fileName,true));
				
					for (int l = 0 ; l < speeds.length ; l++){
						for (int r = 0; r < speeds.length ; r++){
							tmpCmd.newCommand(	dirL*RealLifeProperties.speedToV(speeds[l]),
												dirR*RealLifeProperties.speedToV(speeds[r]));
							double err = calcError(start,tmpCmd).sum();
							this.idealState.setState(idealStartState);
							out.write(err + " ");
							if (err < minErr){
								minErr = err;
								bestL = l;
								bestR = r;
							}
						}
						out.newLine();
					}	

				
				
				out.close();
			} catch (Exception e){
				
			}
			//tmpCmd.newCommand(dirL*RealLifeProperties.speedToV(speeds[bestL]), dirL*RealLifeProperties.speedToV(speeds[bestR]));
			//return tmpCmd;
			
			
		}
		
		
		///////////////////////////////////////////////
		
		bestL = -1;
		bestR = -1;
		minErr = Double.MAX_VALUE;
		
		
		long startTime = System.nanoTime();
		
		
		double bestErr = Double.MAX_VALUE;
		for (int i = 0 ; i < speeds.length ; i++){
			errorTmp = Math.abs(speeds[i]-actL);
			if (errorTmp < bestErr){
				bestErr = errorTmp;
				bestL = i;
			} else {
				break;
			}
		}
		
		actL = bestL;
		
		int startL = actL;
		
		bestErr = Double.MAX_VALUE;
		for (int i = 0 ; i < speeds.length ; i++){
			errorTmp = Math.abs(speeds[i]-actR);
			if (errorTmp < bestErr){
				bestErr = errorTmp;
				bestR = i;
			} else {
				break;
			}
		}
		
		actR = bestR;
		
		int startR = actR;
		
		bestL = -1;
		bestR = -1;
		bestErr = Double.MAX_VALUE;
		
		double[][] errors = new double[3][3];
		while ( bestR != 1 || bestL != 1){
			
			for (int i = 0; i < 3; i++){
				for (int j = 0; j < 3; j++){
					// error calc phase 
					if ( actL+i-1 <0|| actR+j-1 <0 || actL+i-1 >= speeds.length|| actR+j-1 >= speeds.length ){
						errors[i][j] = Double.MAX_VALUE;
					} else {
						if (Math.abs(errors[i][j]) < 0.0001){
							if (i == 1 && j == 1){
								int a = 5;
							}
							tmpCmd.newCommand(	dirL*RealLifeProperties.speedToV(speeds[actL+i-1]),
												dirR*RealLifeProperties.speedToV(speeds[actR+j-1]));
							errors[i][j] = calcError(start,tmpCmd).sum();
							this.idealState.setState(idealStartState);
						}
						
						//find the best
						if (bestErr > errors[i][j]){
							bestErr = errors[i][j];
							bestL = i;
							bestR = j;
						}
					}
				}
			}
			
			if (errors[bestL][bestR] == errors[1][1]){
				break;
			}
			
			// move to the best
			if(bestL == 1 && bestR == 1){
				break;
			}
			
			errors = shift(errors,bestL,bestR);
			actL = actL + bestL - 1;
			actR = actR + bestR - 1;
			bestErr = Double.MAX_VALUE;
			jumps++;
		}
		
		
		tmpCmd.newCommand(	dirL*RealLifeProperties.speedToV(speeds[actL]),
							dirR*RealLifeProperties.speedToV(speeds[actR]));
		
		
		long endTime = System.nanoTime();
		if (Main.surfB && !Main.dec){
			System.out.println();
			System.out.println("Surf output:");

			System.out.println("\tError = " + minErr);
			System.out.println("\tvL = " + dirL*tmpCmd.getRealV(Side.LEFT) + 
								"\n\tvR = " + dirR*tmpCmd.getRealV(Side.RIGHT));
			System.out.println("\tDone in: " + (endTime-startTime)/RealLifeProperties.s2ns +
								" in " + jumps + " jumps.");
			
		}
		
		return tmpCmd;
	}
	
	public RobotCommand evaluate(State start, RobotCommand idealCmd){
		return evaluate(start,idealCmd,new State(start));
	}
	
	private Error calcError(State start, RobotCommand cmd){
		
		Error error = new Error();
		
		calcConstants(start,cmd);
		
		Error errorP1 = calcP1Error(start);
		Error errorP2 = calcP2Error(p1EndState);
		Error errorP3 = calcP3Error(p2EndState);
		
		//error = errorP1.add(errorP2.add(errorP3));
		error = errorP3;
		return error;
	}

	private Error calcP1Error(State start){
		Error error = new Error();
		
		long tStart = System.nanoTime();
		
		long t = t0;
		State actState = new State(start);
		State idealState = this.idealState;
		
		long prevTick = t0;
		long elapsed = 0;
		
		int spL = spLstart;
		int spR = spRstart;
		
		spLp1 = spLend;
		spRp1 = spRend;
		
		long nextLeft = (long) (t + ((double)spL)*RealLifeProperties.s2ns/RealLifeProperties.timerFrequency);
		long nextRight = (long) (t + ((double)spR)*RealLifeProperties.s2ns/RealLifeProperties.timerFrequency);
		
		actState.vL = dirL*RealLifeProperties.speedToV(spLstart);
		actState.vR = dirR*RealLifeProperties.speedToV(spRstart);
		
		int tickCntL = 0;
		int tickCntR = 0;
		
		while ( (spL != spLend || spR != spRend) && nextRight < tEnd && nextLeft < tEnd ){
			
			if (nextLeft > nextRight){
				
				elapsed = nextRight - t;
				t = nextRight;
				
				tickCntR++;
				
				actState = RealRobot.idealMove(actState, elapsed);
				idealState = RealRobot.idealMove(idealState, elapsed);
				
				error.add(	distBetween(actState, idealState)*distBetween(actState, idealState),
							angleSquareBetween(actState, idealState));
					
				if (tickCntR % 2 == 0){
					if (spR < spRend){
						spR++;
					} else {
						spR--;
					}
				}
				
				
				actState.vR = dirR*RealLifeProperties.speedToV(spR);

				
				nextRight = (long) (t + ((double)spR)*RealLifeProperties.s2ns/RealLifeProperties.timerFrequency);
				
			} else {
				
				elapsed = nextLeft-t;
				t = nextLeft;
				
				tickCntL++;
				
				actState = RealRobot.idealMove(actState, elapsed);
				idealState = RealRobot.idealMove(idealState, elapsed);
				
				error.add(	distBetween(actState, idealState),
							angleSquareBetween(actState, idealState));
	
				if (tickCntL % 2 == 0){
					if (spL < spLend){
						spL++;
					} else {
						spL--;
					}
				}
				
				
				actState.vL = dirL*RealLifeProperties.speedToV(spL);

				
				nextLeft = (long) (t + ((double)spL)*RealLifeProperties.s2ns/RealLifeProperties.timerFrequency);
			}
			
			
		}
		
		
		
		if (spLend == spL){
			spRp1 = spR;
		} else {
			spLp1 = spL;
		}
		
		p1EndTime = Math.min(nextRight, nextLeft);
		p1EndState = actState;
		
		//return error;
		return new Error();
	}

	private Error calcP2Error(State start){
		Error error = new Error();
		
		long t = p1EndTime;
		State actState = new State(start);
		State idealState = this.idealState;
		
		long prevTick = t;
		long elapsed = 0;
		
		int sp = 0;
		int spEnd = 0;
		int dir = 0;
		Side accelerating = null;
		
		if (spLend == spLp1){
			sp = spRp1;
			spEnd = spRend;
			accelerating = Side.RIGHT;
			dir = dirR;
		} else {
			sp = spLp1;
			spEnd = spLend;
			accelerating = Side.LEFT;
			dir = dirL;
		}
		

		long nextTick = (long) (t + ((double)sp)*RealLifeProperties.s2ns/RealLifeProperties.timerFrequency);
		
		int tickCnt = 0;
		
		while ( sp != spEnd && t < tEnd){
			
			t = nextTick;
			elapsed = t - prevTick;
			prevTick = nextTick;
			
			tickCnt++;
			
			actState = RealRobot.idealMove(actState, elapsed);
			idealState = RealRobot.idealMove(idealState, elapsed);
			
			error.add(	distBetween(actState, idealState),
						angleSquareBetween(actState, idealState));
			
			if (tickCnt % 2 == 0){
				
				if (sp < spEnd){
					sp++;
				} else {
					sp--;
				}
				
				switch (accelerating){
				case LEFT:
					actState.vL = dirL*RealLifeProperties.speedToV(sp);
					break;
				case RIGHT:
					actState.vR = dirR*RealLifeProperties.speedToV(sp);
				}
				
			}
			
			
			
			nextTick = (long) (t + ((double)sp)*RealLifeProperties.s2ns/RealLifeProperties.timerFrequency);
			
		}
		
		p2EndTime = prevTick;
		p2EndState = actState;
		
		//return error;
		return new Error();
	}
	
	private Error calcP3Error(State start){
		Error error = new Error();
		
		int gran = 1;
		long delta = (tEnd-p2EndTime) / gran;
		
		long t = p2EndTime;
		State actState = new State(start);
		State idealState = this.idealState;
		for (int i = 0; i < gran; i++){
			actState = RealRobot.idealMove(actState, delta);
			idealState = RealRobot.idealMove(idealState, delta);
			if (i == gran-1){
				error.add(	distBetween(actState, idealState),
							angleSquareBetween(actState, idealState)*0);
							
			}
			
			t += delta;

		}
		
		
		return error;
	}
	
	public void writeSurfaceToFile(State start, RobotCommand idealCmd){
		idealStartState = new State(start);
		idealStartState.fi = 0;
		idealStartState.vL = idealCmd.getRealV(Side.LEFT);
		idealStartState.vR = idealCmd.getRealV(Side.RIGHT);
		
		idealState = new State(idealStartState);
		

		// L+ = top
		// R+ = right
		int cmdSpL = idealCmd.getSpeedL();
		int cmdSpR = idealCmd.getSpeedR();

		int jumps = -1;
		
		double bestVal = Double.MAX_VALUE;
		double bestInd = 5;
		
		RobotCommand tmpCmd = new RobotCommand(idealCmd);

		double errorTmp = -1;
		
		long startTime = System.nanoTime();

		String file = "surfaceWsp.txt";
		try {
			BufferedWriter out = new BufferedWriter(new FileWriter(file,true));
			/*
			for (int spR = 9 ; spR < 255 ; spR++){
				for (int spL = 9 ; spL < 255 ; spL++){
					tmpCmd.newCommand(	RealLifeProperties.speedToV(spL),
										RealLifeProperties.speedToV(spR));
					errorTmp = calcError(start,tmpCmd).sum();
					out.write( errorTmp + " " );
					
					idealState.setState(idealStartState);
				}
				out.newLine();
			}
			*/
			
			for (int i = 0; i < speeds.length ; i++){
				for (int j = 0; j < speeds.length ; j++){
					tmpCmd.newCommand(	RealLifeProperties.speedToV(speeds[i]),
							RealLifeProperties.speedToV(speeds[j]));
					errorTmp = calcError(start,tmpCmd).sum();
					out.write( errorTmp + " " );
					
					idealState.setState(idealStartState);
				}
				out.newLine();
			}
			
			out.close();
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		
		
		
		long endTime = System.nanoTime();

		System.out.println("\tDone in: " + (endTime-startTime)/RealLifeProperties.s2ns +
							"in " + jumps + " jumps.");
		
	}
	
	private static double[][] shift(double[][] a, int fromY,int fromX){
		
		if (fromX == 0){
			for (int row = 0; row < 3 ; row++){
				for (int col = 2; col > 0; col--){
					a[row][col] = a[row][col-1];
				}
				a[row][0] = 0;
			}
		} else if (fromX == 2){
			for (int row = 0; row < 3 ; row++){
				for (int col = 0; col < 2; col++){
					a[row][col] = a[row][col+1];
				}
				a[row][2] = 0;
			}
		}
		
		if (fromY == 0){
			for(int col = 0; col < 3; col++){
				for(int row = 2; row > 0; row--){
					a[row][col] = a[row-1][col];
				}
				a[0][col] = 0;
			}
		} else if (fromY == 2){
			for(int col = 0; col < 3; col++){
				for(int row = 0; row < 2; row++){
					a[row][col] = a[row+1][col];
				}
				a[2][col] = 0;
			}
		}
		
		return a;
	}
	
	private static double distBetween(State a, State b){
		double val = 0;
		
		val += Math.pow((a.pos.x-b.pos.x),2) + Math.pow((a.pos.y-b.pos.y),2);
		val = Math.sqrt(val);
		
		return val;
	}
	
	private static double angleSquareBetween(State a, State b){
		return Math.pow(a.fi-b.fi, 2);
	}
	
	private void calcConstants(State start, RobotCommand cmd){
		spLstart = RealLifeProperties.vToSpeed(Math.abs(start.vL));
		spRstart = RealLifeProperties.vToSpeed(Math.abs(start.vR));
		
		spLend = cmd.getSpeedL();
		spRend = cmd.getSpeedR();
		
		accTL = (long) ((t0 + sumFromTo(spLstart,spLend))*RealLifeProperties.s2ns/RealLifeProperties.timerFrequency);
		accTR = (long) ((t0 + sumFromTo(spRstart,spRend))*RealLifeProperties.s2ns/RealLifeProperties.timerFrequency);
		
		//powerL = Math.log(((double)spLstart)/spLend) / Math.log(accTL/t0);
		//powerR = Math.log(((double)spRstart)/spRend) / Math.log(accTR/t0);
		
		//cL = RealLifeProperties.timerFrequency*RealLifeProperties.StepDistanceMM/RealLifeProperties.m2mm;
		//cR = cL;
		//cL /= spLstart * Math.pow(t0, powerL) * 2.0;
		//cR /= spRstart * Math.pow(t0, powerR) * 2.0;
		
		p1EndTime = accTL > accTR ? accTR : accTL;
		p2EndTime = accTL > accTR ? accTL : accTR;
		
		tEnd = (long) (cmd.getRealTime()*RealLifeProperties.s2ns/1000.0);
	}
	
	private double getV(long nanoTime, Side side){
		double val = 0;
		switch (side){
		case LEFT:
			val = cL*Math.pow(nanoTime, powerL);
		case RIGHT:
			val = cR*Math.pow(nanoTime, powerR);
		}
		return val;
	}
	
	private static int sumTo(int n){
		return (int) (n*(n+1)/2.0);
	}
	
	public static int sumFromTo(int from, int to){
		
		int val = 0;
		
		if (from > to){
			val += sumTo(from);
			val -= sumTo(to);
		} else {
			val += sumTo(to);
			val -= sumTo(from);
		}
		
		return val;
	}

	private static class Error{
		private double dist = 0;
		private double orient = 0;
		
		public Error(){
			dist = 0;
			orient = 0;
		}
		
		public double sum(){
			return dist+orient;
		}
		
		public void add(double dist, double orient){
			this.dist += dist;
			this.orient += orient;
		}
	
	}
}
